#ifndef __MATH_ANGLES_H__
#define __MATH_ANGLES_H__

/*
===============================================================================

Euler angles

(to avoid the conversion before trig calls)

All trig calls use float precision

to_Mat3 passes in workspace to avoid a memory_copy

===============================================================================
*/


namespace inf {

    // angle indexes
    static const int PITCH	    = 0;		// up / down
    static const int YAW		= 1;		// left / right
    static const int ROLL		= 2;		// fall over

    static const int A_YAW		= 2;
    static const int A_PITCH	= 1;
    static const int A_ROLL		= 0;

    class Vec3;
    class Quat;
    class Rotation;
    class Mat3;
    class Mat4;

    class Angles {
    public:
        float			pitch;
        float			yaw;
        float			roll;

        Angles( void );
        Angles( float pitch, float yaw, float roll );
        explicit		Angles( const Vec3 &v );

        void 			set( float pitch, float yaw, float roll );
        Angles &		zero( void );

        float			operator[]( int index ) const;
        float &			operator[]( int index );
        Angles			operator-() const;			// negate angles, in general not the inverse rotation
        Angles &		operator=( const Angles &a );
        Angles &		operator=( const Vec3 &a );
        Angles			operator+( const Angles &a ) const;
        Angles			operator+( const Vec3 &a ) const;
        Angles &		operator+=( const Angles &a );
        Angles &		operator+=( const Vec3 &a );
        Angles			operator-( const Angles &a ) const;
        Angles			operator-( const Vec3 &a ) const;
        Angles &		operator-=( const Angles &a );
        Angles &		operator-=( const Vec3 &a );
        Angles			operator*( const float a ) const;
        Angles &		operator*=( const float a );
        Angles			operator/( const float a ) const;
        Angles &		operator/=( const float a );

        friend Angles	operator*( const float a, const Angles &b );
        friend Angles	operator+( const Vec3 &a, const Angles &b );
        friend Angles	operator-( const Vec3 &a, const Angles &b );


        bool			compare( const Angles &a ) const;							// exact compare, no epsilon
        bool			compare( const Angles &a, const float epsilon ) const;	// compare with epsilon
        bool			operator==(	const Angles &a ) const;						// exact compare, no epsilon
        bool			operator!=(	const Angles &a ) const;						// exact compare, no epsilon

        Angles &		normalize_360( void );	// normalizes 'this'
        Angles &		normalize_180( void );	// normalizes 'this'

        void			clamp( const Angles &min, const Angles &max );

        int				get_dimension( void ) const;

        void			to_Vectors( Vec3 *forward, Vec3 *right = NULL, Vec3 *up = NULL ) const;
        Vec3			to_forward( void ) const;
        Quat			to_Quat( void ) const;
        Rotation		to_Rotation( void ) const;
        Mat3			to_Mat3( void ) const;
        Mat3&			to_Mat3( Mat3& mat ) const;
        void			to_Mat3_no_roll( Mat3& mat ) const;
        static Mat3&	yaw_to_Mat3( float yaw, Mat3& mat );
        static Mat3&	pitch_to_Mat3( float pitch, Mat3& mat );
        static Mat3&	roll_to_Mat3( float roll, Mat3& mat );
        Mat4			to_Mat4( void ) const;
        Vec3			to_angular_velocity( void ) const;
        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

        Mat3			to_Mat3_Maya( void ) const;

        bool			fix_denormals( float epsilon = math::FLOAT_EPSILON );		// change tiny numbers to zero

        static Angles ZERO;
    };


    INF_INLINE Angles::Angles( void ) {
    }

    INF_INLINE Angles::Angles( float pitch, float yaw, float roll ) {
        this->pitch = pitch;
        this->yaw	= yaw;
        this->roll	= roll;
    }

    INF_INLINE Angles::Angles( const Vec3 &v ) {
        this->pitch = v[0];
        this->yaw	= v[1];
        this->roll	= v[2];
    }

    INF_INLINE void Angles::set( float pitch, float yaw, float roll ) {
        this->pitch = pitch;
        this->yaw	= yaw;
        this->roll	= roll;
    }

    INF_INLINE Angles &Angles::zero( void ) {
        pitch = yaw = roll = 0.0f;
        return *this;
    }

    INF_INLINE float Angles::operator[]( int index ) const {
        assert( ( index >= 0 ) && ( index < 3 ) );
        return ( &pitch )[ index ];
    }

    INF_INLINE float &Angles::operator[]( int index ) {
        assert( ( index >= 0 ) && ( index < 3 ) );
        return ( &pitch )[ index ];
    }

    INF_INLINE Angles Angles::operator-( void ) const {
        return Angles( -pitch, -yaw, -roll );
    }

    INF_INLINE Angles &Angles::operator=( const Angles &a ) {
        pitch	= a.pitch;
        yaw		= a.yaw;
        roll	= a.roll;
        return *this;
    }

    INF_INLINE Angles &Angles::operator=( const Vec3 &a ) 
    {
        pitch = a.x;
        yaw = a.y;
        roll = a.z;
        return *this;
    }

    INF_INLINE Angles Angles::operator+( const Angles &a ) const {
        return Angles( pitch + a.pitch, yaw + a.yaw, roll + a.roll );
    }

    INF_INLINE Angles Angles::operator+( const Vec3 &a ) const 
    {
        return Angles( pitch + a.x, yaw + a.y, roll + a.z );
    }

    INF_INLINE Angles& Angles::operator+=( const Angles &a ) {
        pitch	+= a.pitch;
        yaw		+= a.yaw;
        roll	+= a.roll;

        return *this;
    }

    INF_INLINE Angles& Angles::operator+=( const Vec3 &a ) 
    {
        pitch += a.x;
        yaw += a.y;
        roll += a.z;
        return *this ;
    }

    INF_INLINE Angles Angles::operator-( const Angles &a ) const {
        return Angles( pitch - a.pitch, yaw - a.yaw, roll - a.roll );
    }

    INF_INLINE Angles& Angles::operator-=( const Angles &a ) {
        pitch	-= a.pitch;
        yaw		-= a.yaw;
        roll	-= a.roll;

        return *this;
    }

    INF_INLINE Angles& Angles::operator-=( const Vec3 &a ) 
    {
        pitch -= a.x;
        yaw -= a.y;
        roll -= a.z;
        return *this;
    }

    INF_INLINE Angles Angles::operator*( const float a ) const {
        return Angles( pitch * a, yaw * a, roll * a );
    }

    INF_INLINE Angles& Angles::operator*=( float a ) {
        pitch	*= a;
        yaw		*= a;
        roll	*= a;
        return *this;
    }

    INF_INLINE Angles Angles::operator/( const float a ) const {
        float inva = 1.0f / a;
        return Angles( pitch * inva, yaw * inva, roll * inva );
    }

    INF_INLINE Angles& Angles::operator/=( float a ) {
        float inva = 1.0f / a;
        pitch	*= inva;
        yaw		*= inva;
        roll	*= inva;
        return *this;
    }

    INF_INLINE Angles operator*( const float a, const Angles &b ) {
        return Angles( a * b.pitch, a * b.yaw, a * b.roll );
    }

    INF_INLINE Angles operator+( const Vec3 &a, const Angles &b ) 
    {
        return Angles( a.x + b.pitch, a.y + b.yaw, a.z + b.roll );
    }

    INF_INLINE Angles operator-( const Vec3 &a, const Angles &b ) 
    {
        return Angles( a.x - b.pitch, a.y - b.yaw, a.z - b.roll ); 
    }

    INF_INLINE bool Angles::compare( const Angles &a ) const {
        return ( ( a.pitch == pitch ) && ( a.yaw == yaw ) && ( a.roll == roll ) );
    }

    INF_INLINE bool Angles::compare( const Angles &a, const float epsilon ) const {
        if ( math::fabs( pitch - a.pitch ) > epsilon ) {
            return false;
        }

        if ( math::fabs( yaw - a.yaw ) > epsilon ) {
            return false;
        }

        if ( math::fabs( roll - a.roll ) > epsilon ) {
            return false;
        }

        return true;
    }

    INF_INLINE bool Angles::operator==( const Angles &a ) const {
        return compare( a );
    }

    INF_INLINE bool Angles::operator!=( const Angles &a ) const {
        return !compare( a );
    }

    INF_INLINE void Angles::clamp( const Angles &min, const Angles &max ) {
        if ( pitch < min.pitch ) {
            pitch = min.pitch;
        } else if ( pitch > max.pitch ) {
            pitch = max.pitch;
        }
        if ( yaw < min.yaw ) {
            yaw = min.yaw;
        } else if ( yaw > max.yaw ) {
            yaw = max.yaw;
        }
        if ( roll < min.roll ) {
            roll = min.roll;
        } else if ( roll > max.roll ) {
            roll = max.roll;
        }
    }

    INF_INLINE int Angles::get_dimension( void ) const {
        return 3;
    }

    INF_INLINE Mat3& Angles::to_Mat3( Mat3& mat ) const {
        float	sr, sp, sy, cr, cp, cy;

        math::sincos( yaw, sy, cy );
        math::sincos( pitch, sp, cp );
        math::sincos( roll, sr, cr );

        mat[0].set( cp * cy, cp * sy, -sp );
        mat[1].set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
        mat[2].set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );

        return( mat );
    }

    INF_INLINE const float *Angles::to_float_ptr( void ) const {
        return &pitch;
    }

    INF_INLINE float *Angles::to_float_ptr( void ) {
        return &pitch;
    }

    INF_INLINE Mat3& Angles::yaw_to_Mat3( float yaw, Mat3& mat ) {
        float sy, cy;

        math::sincos( math::DEG2RAD( yaw ), sy, cy );

        mat[ 0 ].set( cy, sy, 0 );
        mat[ 1 ].set( -sy, cy, 0 );
        mat[ 2 ].set( 0, 0, 1 );

        return mat;
    }

    INF_INLINE Mat3& Angles::pitch_to_Mat3( float pitch, Mat3& mat ) {
        float sp, cp;

        math::sincos( math::DEG2RAD( pitch ), sp, cp );

        mat[ 0 ].set( cp, 0, -sp );
        mat[ 1 ].set( 0, 1, 0 );
        mat[ 2 ].set( sp, 0, cp );

        return mat;
    }

    INF_INLINE Mat3& Angles::roll_to_Mat3( float roll, Mat3& mat ) {
        float sr, cr;

        math::sincos( math::DEG2RAD( roll ), sr, cr );

        mat[ 0 ].set( 1, 0, 0 );
        mat[ 1 ].set( 0, cr, sr );
        mat[ 2 ].set( 0, -sr, cr );

        return mat;
    }

    INF_INLINE bool Angles::fix_denormals( float epsilon ) {
        bool denormal = false;
        if ( math::fabs( yaw ) < epsilon ) {
            yaw = 0.0f;
            denormal = true;
        }
        if ( math::fabs( pitch ) < epsilon ) {
            pitch = 0.0f;
            denormal = true;
        }
        if ( math::fabs( roll ) < epsilon ) {
            roll = 0.0f;
            denormal = true;
        }
        return denormal;
    }


}   // ::inf

#endif /* !__MATH_ANGLES_H__ */
